#include "include.h"

void *thread_acceleration(void *thread_id)
{
	int start, end, increment;
	int id = (int)(intptr_t)thread_id; // 获取线程ID

	while (1)
	{
		increment = par_num / th_num; // 计算每个线程处理的数据块大小
		start = id * increment;		  // 计算起始位置
		end = start + increment;	  // 计算结束位置
		if (id == th_num - 1)
		{
			end = par_num; // 最后一个线程处理剩余的数据
		}

		pthread_mutex_lock(&th_mutex[id]);
		while (!th_flag[id])
		{
			pthread_cond_wait(&th_cond[id], &th_mutex[id]); // 等待条件变量th_cond[id]满足
		}
		pthread_mutex_unlock(&th_mutex[id]);

		calculate_acceleration(start, end); // 计算加速度
		th_flag[id] = 0;					// 重置线程标志位为0
	}
}

void *thread_location(void *thread_id)
{
	while (1)
	{
		pthread_mutex_lock(&mutex0);
		while (!flag0)
		{
			pthread_cond_wait(&cond0, &mutex0); // 等待条件变量cond0满足
		}
		pthread_mutex_unlock(&mutex0);

		calculate_location(); // 计算位置

		flag0 = 0;	// 重置速度位置标志位为0
		run_cnt0++; // 运行计数加一
	}
}

void *thread_cmd(void *thread_id)
{
	while (1)
	{
		usleep(TIME_DT * (1000 - 10) * 1000); // 线程休眠TIME_DT秒

		while (1)
		{
			int flag_cnt = 0;
			for (int i = 0; i < th_num; i++)
			{
				flag_cnt += th_flag[i]; // 统计运行完毕标志位为1的线程数量
			}
			if (flag_cnt == 0 && flag0 == 0) // 如果所有线程标志位为0且速度位置标志位为0,则跳出循环
			{
				break;
			}
			usleep(100); // 等待100微秒
		}

		// 激活各个线程
		for (int i = 0; i < th_num; i++)
		{
			pthread_mutex_lock(&th_mutex[i]);
			th_flag[i] = 1;					  // 设置对应线程标志位为1
			pthread_cond_signal(&th_cond[i]); // 发送条件信号
			pthread_mutex_unlock(&th_mutex[i]);
		}

		pthread_mutex_lock(&mutex0);
		flag0 = 1;					 // 设置速度位置标志位为1
		pthread_cond_signal(&cond0); // 发送条件信号
		pthread_mutex_unlock(&mutex0);
	}
}

void thread_init(void)
{
	pthread_t thread1[TH_MAX], thread2, thread3; // 声明线程变量

	// 初始化线程互斥锁、条件变量和标志
	for (int i = 0; i < th_num; i++)
	{
		pthread_mutex_init(&th_mutex[i], NULL);
		pthread_cond_init(&th_cond[i], NULL);
		th_flag[i] = 0;
	}

	// 创建并启动加速度计算线程 主要的计算量
	for (int i = 0; i < th_num; i++)
	{
		pthread_create(&thread1[i], NULL, thread_acceleration, (void *)(intptr_t)i);
	}

	pthread_create(&thread2, NULL, thread_location, NULL); // 创建计算速度与位置线程
	pthread_create(&thread3, NULL, thread_cmd, NULL);	   // 创建发送信号线程
}